/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stm32f1xx_hal.h"
#include "led.h"
#include <stdio.h>
#include "systemfunc.h"
#include "motor.h"
#include "mt6701ct_spi.h"
#include <math.h>
#include "../pid/pid.h"
#define POLE_PAIRS 7

uint16_t duty = 350; // 50% 占空比（假设ARR=1000）
static uint16_t step_count = 0;
extern uint8_t rx_byte ;
void SystemClock_Config(void);
//extern float PID_Compute(PID_t *pid, float target, float measured);

PID_t speed_pid = {
  .kp = 4.0f,  // 增大比例增益，提高系统响应速度
  .ki = 0.5f,  // 减小积分增益，避免过度积累导致震荡
  .kd = 0.5f   // 增大微分增益，增加系统阻尼，减少过冲
};

// 定义方向标志
typedef enum {
  FORWARD = 1, // 正转
  REVERSE = -1 // 反转
} MotorDirection;

MotorDirection motor_direction = FORWARD; // 默认正转

// 控制电机的正反转
void set_motor_direction(MotorDirection direction) {
  motor_direction = direction;
}

/**
* 定时器pwm波形初始化
*/
void pwm_init(void)
{
  TIM_OC_InitTypeDef sConfigOC = {0};

  htim1.Instance = TIM1;
  htim1.Init.Prescaler = 72 - 1;         // 72MHz / 72 = 1MHz
  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim1.Init.Period = 1000 - 1;          // PWM频率 = 1MHz / 1000 = 1kHz
  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  HAL_TIM_PWM_Init(&htim1);

  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;  // 初始占空比为0
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;

  HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); // PA8
  HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2); // PA9
  HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_3); // PA10
}

/**
* 六步换向核心代码
*/


void custom_motor_commutate(uint8_t step, uint16_t duty) {
    // 清除所有通道占空比
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);

    // 所有引脚先拉低（低边关闭）
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET);

    if (motor_direction == FORWARD) {
        // 正转：1→2→3→4→5→6（U→V→W相依次导通）
        switch (step) {
            case 1: // U+ W-（PA8高，PA10低）
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty); // PA8 PWM（U相上桥臂）
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET); // PA10低（W相下桥臂）
                break;
            case 2: // U+ V-（PA8高，PA9低）
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty); // PA8 PWM
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET); // PA9低（V相下桥臂）
                break;
            case 3: // V+ U-（PA9高，PA8低）
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty); // PA9 PWM（V相上桥臂）
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET); // PA8低（U相下桥臂）
                break;
            case 4: // V+ W-（PA9高，PA10低）
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty); // PA9 PWM
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET); // PA10低
                break;
            case 5: // W+ V-（PA10高，PA9低）
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty); // PA10 PWM（W相上桥臂）
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET); // PA9低
                break;
            case 6: // W+ U-（PA10高，PA8低）
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty); // PA10 PWM
                HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET); // PA8低
                break;
            default:
                break;
        }
    } else if (motor_direction == REVERSE) {
        // 反转时，换向表镜像正向
        // 反转逻辑：步序为正转的逆序（step1对应正转step6，step2对应step5，依此类推）
        switch (step) {
            case 1: // 对应正转step6：W+ U-
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty); // W相上桥臂（PA10）
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET); // U相下桥臂（PA8）
            break;
            case 2: // 对应正转step5：W+ V-
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty); // W相上桥臂
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET); // V相下桥臂（PA9）
            break;
            case 3: // 对应正转step4：V+ W-
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty); // V相上桥臂（PA9）
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET); // W相下桥臂（PA10）
            break;
            case 4: // 对应正转step3：V+ U-
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, duty); // V相上桥臂
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET); // U相下桥臂
            break;
            case 5: // 对应正转step2：U+ V-
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty); // U相上桥臂（PA8）
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_9, GPIO_PIN_RESET); // V相下桥臂
            break;
            case 6: // 对应正转step1：U+ W-
                __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty); // U相上桥臂
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET); // W相下桥臂
            break;
            default:
                break;
        }
    }
}

/**
* 循环 根绝检测到的位置信息，计算换向步 然后设置到换向函数中
*/
void loop() {
    float mech_deg = get_absolute_position();
    float elec_deg = fmod(mech_deg * POLE_PAIRS+90, 360.0f);
    int step = get_commutation_step(elec_deg);
    custom_motor_commutate(step, duty);
    HAL_Delay(1);
    step_count++;

    float speed=get_motor_speed_rpm();
    if(step_count%1000==0){
      printf("speed=%.2f\n", speed);
    }
}

void motor_open_loop_start()
{
    set_motor_direction(FORWARD); // 设置转向
    for (uint8_t i = 0; i < 36; i++) {
        custom_motor_commutate((i % 6) + 1, 300);  // 固定占空比换相
        HAL_Delay(3); // 足够时间让电机慢慢转起来
    }
}


void speed_control_loop()
{
  float target_rps = 6.0f; // 期望5转每秒
  float actual_rps = get_motor_speed_rps();

  float control = PID_Compute(&speed_pid, target_rps, actual_rps);

  // 限制输出范围
  if (control > 500) control = 500;
  if (control < 0) control = 0;

  // 使用六步换向控制电机（每次更新PWM占空比）
  float mech_deg = get_absolute_position();
  float elec_deg = fmod(mech_deg * POLE_PAIRS+90, 360.0f);
  int step = get_commutation_step(elec_deg);
  // 设置电机转向
  set_motor_direction(FORWARD);  // 正转
  custom_motor_commutate(step, (uint16_t)control);  // 按照当前步进执行换向

  HAL_Delay(1);
  step_count++;
  if(step_count%1000==0){
    float speed=get_motor_speed_rps();
    printf("speed=%.2f\n", speed);
    float rings=MT6701_GetTotalRotations();
    printf("rings=%.2f\n", rings);
  }
}

int startup_flag=1;
int main(void)
{
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_USART2_UART_Init();
  //MX_TIM1_Init();
  pwm_init();
  MX_SPI1_Init();

  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);

  // 初始化 GPIO
  Motor_MX_GPIO_Init();
  Motor_MX_TIM1_Init();

  //串口使能
  HAL_UART_Receive_IT(&huart2, &rx_byte, 1);

  if (startup_flag == 1) {
      motor_open_loop_start();
      startup_flag = 0; // 标记已完成启动
  }

  while (1)
  {
    //开环控制
    //loop();

    //pid 闭环控制
    speed_control_loop();

  }
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}


void Error_Handler(void)
{
  __disable_irq();
  while (1)
  {
  }
}
